/*----------------------------------------------------------------------------
 * Tencent is pleased to support the open source community by making TencentOS
 * available.
 *
 * Copyright (C) 2019 THL A29 Limited, a Tencent company. All rights reserved.
 * If you have downloaded a copy of the TencentOS binary from Tencent, please
 * note that the TencentOS binary is licensed under the BSD 3-Clause License.
 *
 * If you have downloaded a copy of the TencentOS source code from Tencent,
 * please note that TencentOS source code is licensed under the BSD 3-Clause
 * License, except for the third-party components listed below which are
 * subject to different license terms. Your integration of TencentOS into your
 * own projects may require compliance with the BSD 3-Clause License, as well
 * as the other licenses applicable to the third-party components included
 * within TencentOS.
 *---------------------------------------------------------------------------*/

#include "tos_k.h"

__STATIC_INLINE__ void prio_q_item_copy_to(k_prio_q_t *prio_q, void *item_out,
		size_t *item_size, prio_q_slot_t slot)
{
	memcpy(item_out, PRIO_Q_THE_ITEM(prio_q, slot), prio_q->item_size);
	if (item_size)
	{
		*item_size = prio_q->item_size;
	}
}

__STATIC_INLINE__ void prio_q_item_copy_from(k_prio_q_t *prio_q, void *item_in,
		prio_q_slot_t slot)
{
	memcpy(PRIO_Q_THE_ITEM(prio_q, slot), item_in, prio_q->item_size);
}

__STATIC_INLINE__ void prio_q_item_increase(k_prio_q_t *prio_q)
{
	++prio_q->total;
}

__STATIC_INLINE__ void prio_q_item_decrease(k_prio_q_t *prio_q)
{
	--prio_q->total;
}

__STATIC__ void prio_q_pool_mgr_init(prio_q_pool_mgr_t *pool_mgr,
		prio_q_pool_mgr_ent_t *pool_mgr_ent_array, size_t item_cnt)
{
	prio_q_slot_t i;

	pool_mgr->first_free = (prio_q_slot_t) 0u;
	pool_mgr->pool_mgr_ent_array = pool_mgr_ent_array;

	for (i = 0; i < item_cnt; ++i)
	{
		pool_mgr_ent_array[i].next = i + 1;
	}
	pool_mgr_ent_array[item_cnt - 1].next = PRIO_Q_POOL_SLOT_INVALID;
}

__STATIC__ void prio_q_pool_mgr_reset(prio_q_pool_mgr_t *pool_mgr,
		size_t item_cnt)
{
	prio_q_slot_t i;
	prio_q_pool_mgr_ent_t *pool_mgr_ent_array;

	pool_mgr->first_free = (prio_q_slot_t) 0u;
	pool_mgr_ent_array = pool_mgr->pool_mgr_ent_array;

	for (i = 0; i < item_cnt; ++i)
	{
		pool_mgr_ent_array[i].next = i + 1;
	}
	pool_mgr_ent_array[item_cnt - 1].next = PRIO_Q_POOL_SLOT_INVALID;
}

__STATIC__ void prio_q_pool_mgr_deinit(prio_q_pool_mgr_t *pool_mgr)
{
	pool_mgr->first_free = (prio_q_slot_t) 0u;
	pool_mgr->pool_mgr_ent_array = K_NULL;
}

__STATIC__ int prio_q_mgr_entry_cmp(void *first, void *second)
{
	prio_q_prio_mgr_ent_t *first_entry, *second_entry;

	first_entry = (prio_q_prio_mgr_ent_t *) first;
	second_entry = (prio_q_prio_mgr_ent_t *) second;

	// numerically bigger, actually smaller, we build a minimal binary heap here
	if (first_entry->priority < second_entry->priority)
	{
		return K_TRUE;
	}
	return K_FALSE;
}

__STATIC__ void prio_q_prio_mgr_init(prio_q_prio_mgr_t *prio_mgr,
		prio_q_prio_mgr_ent_t *prio_mgr_ent_pool, size_t item_cnt)
{
	prio_mgr->prio_mgr_ent_pool = prio_mgr_ent_pool;
	tos_bin_heap_create(&prio_mgr->prio_mgr_bin_heap, prio_mgr_ent_pool,
			item_cnt, sizeof(prio_q_prio_mgr_ent_t), prio_q_mgr_entry_cmp);
}

__STATIC__ void prio_q_prio_mgr_reset(prio_q_prio_mgr_t *prio_mgr)
{
	tos_bin_heap_flush(&prio_mgr->prio_mgr_bin_heap);
}

__STATIC__ void prio_q_prio_mgr_deinit(prio_q_prio_mgr_t *prio_mgr)
{
	prio_mgr->prio_mgr_ent_pool = K_NULL;
	tos_bin_heap_destroy(&prio_mgr->prio_mgr_bin_heap);
}

__STATIC__ prio_q_slot_t prio_q_pool_mgr_slot_alloc(prio_q_pool_mgr_t *pool_mgr)
{
	prio_q_slot_t fresh;
	prio_q_pool_mgr_ent_t *first_free;

	if (pool_mgr->first_free == PRIO_Q_POOL_SLOT_INVALID)
	{
		return PRIO_Q_POOL_SLOT_INVALID;
	}

	fresh = pool_mgr->first_free;
	first_free = &pool_mgr->pool_mgr_ent_array[pool_mgr->first_free];
	pool_mgr->first_free = first_free->next;

	return fresh;
}

__STATIC__ void prio_q_pool_mgr_slot_free(prio_q_pool_mgr_t *pool_mgr,
		prio_q_slot_t slot)
{
	prio_q_pool_mgr_ent_t *slot_entry;

	slot_entry = &pool_mgr->pool_mgr_ent_array[slot];
	slot_entry->next = pool_mgr->first_free;
	pool_mgr->first_free = slot;
}

__STATIC__ void prio_q_prio_mgr_slot_enqueue(prio_q_prio_mgr_t *prio_mgr,
		prio_q_slot_t slot, k_prio_t prio)
{
	k_err_t err;
	prio_q_prio_mgr_ent_t prio_mgr_entry;

	prio_mgr_entry.priority = prio;
	prio_mgr_entry.slot = slot;

	err = tos_bin_heap_push(&prio_mgr->prio_mgr_bin_heap, &prio_mgr_entry,
			sizeof(prio_q_prio_mgr_ent_t));
	TOS_ASSERT(err == K_ERR_NONE);
}

__STATIC__ prio_q_slot_t prio_q_prio_mgr_slot_dequeue(
		prio_q_prio_mgr_t *prio_mgr, k_prio_t *prio)
{
	k_err_t err;
	size_t dummy;
	prio_q_prio_mgr_ent_t prio_mgr_entry;

	err = tos_bin_heap_pop(&prio_mgr->prio_mgr_bin_heap, &prio_mgr_entry,
			&dummy);
	TOS_ASSERT(err == K_ERR_NONE);
	TOS_ASSERT(dummy == sizeof(prio_q_prio_mgr_ent_t));

	if (prio)
	{
		*prio = prio_mgr_entry.priority;
	}

	return prio_mgr_entry.slot;
}

__API__ k_err_t tos_prio_q_create(k_prio_q_t *prio_q, void *mgr_array,
		void *pool, size_t item_cnt, size_t item_size)
{
	prio_q_pool_mgr_ent_t *pool_mgr_ent_array;
	prio_q_prio_mgr_ent_t *prio_mgr_ent_pool;

	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_PTR_SANITY_CHECK(mgr_array);
	TOS_PTR_SANITY_CHECK(pool);

	pool_mgr_ent_array = (prio_q_pool_mgr_ent_t *) mgr_array;
	prio_mgr_ent_pool = (prio_q_prio_mgr_ent_t *) ((uint8_t *) mgr_array
			+ PRIO_Q_POOL_MGR_ENT_ARRAY_SIZE(item_cnt));

	prio_q_pool_mgr_init(&prio_q->pool_mgr, pool_mgr_ent_array, item_cnt);
	prio_q_prio_mgr_init(&prio_q->prio_mgr, prio_mgr_ent_pool, item_cnt);

	prio_q->total = 0;
	prio_q->item_size = item_size;
	prio_q->item_cnt = item_cnt;
	prio_q->mgr_pool = (uint8_t *) mgr_array;
	prio_q->data_pool = (uint8_t *) pool;

	TOS_OBJ_INIT(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);
	knl_object_alloc_set_static(&prio_q->knl_obj);

	return K_ERR_NONE;
}

__API__ k_err_t tos_prio_q_destroy(k_prio_q_t *prio_q)
{
	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_OBJ_VERIFY(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);

	if (!knl_object_alloc_is_static(&prio_q->knl_obj))
	{
		return K_ERR_OBJ_INVALID_ALLOC_TYPE;
	}

	prio_q_pool_mgr_deinit(&prio_q->pool_mgr);
	prio_q_prio_mgr_deinit(&prio_q->prio_mgr);

	prio_q->total = 0;
	prio_q->item_size = 0;
	prio_q->item_cnt = 0;
	prio_q->mgr_pool = K_NULL;
	prio_q->data_pool = K_NULL;

	TOS_OBJ_DEINIT(prio_q);
	knl_object_alloc_reset(&prio_q->knl_obj);

	return K_ERR_NONE;
}

__API__ k_err_t tos_prio_q_create_dyn(k_prio_q_t *prio_q, size_t item_cnt,
		size_t item_size)
{
	k_err_t err;
	void *mgr_pool, *data_pool;

	TOS_PTR_SANITY_CHECK(prio_q);

	mgr_pool = tos_mmheap_alloc(TOS_PRIO_Q_MGR_ARRAY_SIZE(item_cnt));
	if (!mgr_pool)
	{
		return K_ERR_OUT_OF_MEMORY;
	}

	data_pool = tos_mmheap_alloc(item_cnt * item_size);
	if (!data_pool)
	{
		tos_mmheap_free(mgr_pool);
		return K_ERR_OUT_OF_MEMORY;
	}

	err = tos_prio_q_create(prio_q, mgr_pool, data_pool, item_cnt, item_size);
	if (err != K_ERR_NONE)
	{
		tos_mmheap_free(data_pool);
		tos_mmheap_free(mgr_pool);
	}

	knl_object_alloc_set_dynamic(&prio_q->knl_obj);

	return K_ERR_NONE;
}

__API__ k_err_t tos_prio_q_destroy_dyn(k_prio_q_t *prio_q)
{
	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_OBJ_VERIFY(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);

	if (!knl_object_alloc_is_dynamic(&prio_q->knl_obj))
	{
		return K_ERR_OBJ_INVALID_ALLOC_TYPE;
	}

	prio_q_pool_mgr_deinit(&prio_q->pool_mgr);
	prio_q_prio_mgr_deinit(&prio_q->prio_mgr);

	tos_mmheap_free(prio_q->mgr_pool);
	tos_mmheap_free(prio_q->data_pool);

	prio_q->total = 0;
	prio_q->item_size = 0;
	prio_q->item_cnt = 0;
	prio_q->mgr_pool = K_NULL;
	prio_q->data_pool = K_NULL;

	TOS_OBJ_DEINIT(prio_q);
	knl_object_alloc_reset(&prio_q->knl_obj);

	return K_ERR_NONE;
}

__STATIC__ void prio_q_do_enqueue(k_prio_q_t *prio_q, void *item,
		prio_q_slot_t slot, k_prio_t prio)
{
	prio_q_item_copy_from(prio_q, item, slot);
	prio_q_prio_mgr_slot_enqueue(&prio_q->prio_mgr, slot, prio);
	prio_q_item_increase(prio_q);
}

__API__ k_err_t tos_prio_q_enqueue(k_prio_q_t *prio_q, void *item,
		size_t item_size, k_prio_t prio)
{
	TOS_CPU_CPSR_ALLOC();
	prio_q_slot_t the_slot;

	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_PTR_SANITY_CHECK(item);
	TOS_OBJ_VERIFY(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);

	if (item_size != prio_q->item_size)
	{
		return K_ERR_PRIO_Q_ITEM_SIZE_NOT_MATCH;
	}

	if (tos_prio_q_is_full(prio_q))
	{
		return K_ERR_PRIO_Q_FULL;
	}

	TOS_CPU_INT_DISABLE()
	;

	the_slot = prio_q_pool_mgr_slot_alloc(&prio_q->pool_mgr);
	TOS_ASSERT(the_slot != PRIO_Q_POOL_SLOT_INVALID);
	prio_q_do_enqueue(prio_q, item, the_slot, prio);

	TOS_CPU_INT_ENABLE()
	;
	return K_ERR_NONE;
}

__STATIC__ void prio_q_do_dequeue(k_prio_q_t *prio_q, void *item,
		size_t *item_size, prio_q_slot_t slot)
{
	prio_q_pool_mgr_slot_free(&prio_q->pool_mgr, slot);
	prio_q_item_copy_to(prio_q, item, item_size, slot);
	prio_q_item_decrease(prio_q);
}

__API__ k_err_t tos_prio_q_dequeue(k_prio_q_t *prio_q, void *item,
		size_t *item_size, k_prio_t *prio)
{
	TOS_CPU_CPSR_ALLOC();
	prio_q_slot_t the_slot;

	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_PTR_SANITY_CHECK(item);
	TOS_OBJ_VERIFY(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);

	if (tos_prio_q_is_empty(prio_q))
	{
		return K_ERR_PRIO_Q_EMPTY;
	}

	TOS_CPU_INT_DISABLE()
	;

	the_slot = prio_q_prio_mgr_slot_dequeue(&prio_q->prio_mgr, prio);
	prio_q_do_dequeue(prio_q, item, item_size, the_slot);

	TOS_CPU_INT_ENABLE()
	;
	return K_ERR_NONE;
}

__API__ k_err_t tos_prio_q_flush(k_prio_q_t *prio_q)
{
	TOS_CPU_CPSR_ALLOC();

	TOS_PTR_SANITY_CHECK(prio_q);
	TOS_OBJ_VERIFY(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE);

	TOS_CPU_INT_DISABLE()
	;

	prio_q_pool_mgr_reset(&prio_q->pool_mgr, prio_q->item_cnt);
	prio_q_prio_mgr_reset(&prio_q->prio_mgr);
	prio_q->total = 0;

	TOS_CPU_INT_ENABLE()
	;
	return K_ERR_NONE;
}

__API__ int tos_prio_q_is_empty(k_prio_q_t *prio_q)
{
	TOS_CPU_CPSR_ALLOC();
	int is_empty = K_FALSE;

	TOS_PTR_SANITY_CHECK_RC(prio_q, K_FALSE);
	TOS_OBJ_VERIFY_RC(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE, K_FALSE);

	TOS_CPU_INT_DISABLE()
	;
	is_empty = (prio_q->total == 0);
	TOS_CPU_INT_ENABLE()
	;

	return is_empty;
}

__API__ int tos_prio_q_is_full(k_prio_q_t *prio_q)
{
	TOS_CPU_CPSR_ALLOC();
	int is_full = K_FALSE;

	TOS_PTR_SANITY_CHECK_RC(prio_q, K_FALSE);
	TOS_OBJ_VERIFY_RC(prio_q, KNL_OBJ_TYPE_PRIORITY_QUEUE, K_FALSE);

	TOS_CPU_INT_DISABLE()
	;
	is_full = (prio_q->total == prio_q->item_cnt);
	TOS_CPU_INT_ENABLE()
	;

	return is_full;
}

